import sensor, time,lcd
from button import BUTTON

lcd.init()
#sd.deinit()
#lcd.init(type=2,width=240,height=320,rotate=2,invert=1)
button=BUTTON()  #声明按键，梦飞openmv只有一个按键，因此直接内部指定了按键

global THRESHOLD
THRESHOLD  =[ (53, 99, -13, 46, 29, 57)]
last_blobs=None
#功能：寻找最大的色块，计算方式为像素面积排序
#输入： 色块链表blobs，中心坐标X(blob[0]),中心坐标y(blob[1]),（像素宽）blob[2],（像素高）blob[3],
#输出： 最大色块max_blob
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

def auto_color_mask(img):
    global THRESHOLD
    LAB_ERROR=15
    ROI=(int(img.width()/2-10),int(img.height()/2-10),20,20)
    if button.event():
        button.key_event=0
        while not button.event():
            img = sensor.snapshot()
            img.draw_rectangle(ROI)
            lcd.display(img)
        if button.event():
            button.key_event=0
            img = sensor.snapshot()
            statistics=img.get_statistics(roi=ROI)
            img.draw_rectangle(ROI)
            color_l=statistics.l_mode()
            color_a=statistics.a_mode()
            color_b=statistics.b_mode()
            THRESHOLD=[(color_l - LAB_ERROR,color_l + LAB_ERROR,color_a - LAB_ERROR,color_a + LAB_ERROR,color_b - LAB_ERROR,color_b + LAB_ERROR)]
        print(THRESHOLD)

def color_detect(img):
    global last_blobs
    ball_s=0
    blobs=list()
    if last_blobs:#如果上一张图像中找到了色块，就在色块周边扩展，在扩展后的ROI区域进行寻找色块，这样有利于追踪
        b = find_max(last_blobs)#每次只追踪面积最大的色块目标
        roi2 = (b[0]-7,b[1]-7,b[2]+12,b[3]+12)
        # print(roi2)
        blobs = img.find_blobs(THRESHOLD,roi = roi2,area_threshold=100)
        last_blobs = blobs
    else: #如果没有找到色块，就重新计算全图的色块，并将全图色块信息保存到上一张图像上，这样下一次就可以再进行ROI区域寻找色块
        blobs = img.find_blobs(THRESHOLD, area_threshold=100)
        last_blobs = blobs
    if blobs:
        max_blob = find_max(blobs)
        ball_s=10500/((max_blob[2]*2)) #计算距离 #不同的目标需要调整测距参数
        img.draw_rectangle(max_blob.rect())
        img.draw_cross(max_blob.cx(), max_blob.cy())
        img.draw_string(max_blob.cx(), max_blob.cy(), "%.2f mm"%(ball_s))#显示目标的距离参数
############################摄像头初始化部分###########################
sensor.reset() # Initialize the camera sensor.
#sensor.__write_reg(0x11,0x81) #图像传感器的帧率控制，80为30帧，81为15帧
sensor.set_pixformat(sensor.RGB565) # use RGB565.彩色图像
sensor.set_framesize(sensor.V240X240)
sensor.skip_frames(time=1000) # Let new settings take affect.
#sensor.set_hmirror(True) #水平镜像，暂时不用
#sensor.set_vflip(True)  #垂直镜像，根据摄像头的安装位置调整
sensor.set_auto_whitebal(False) # 除7670以外其他摄像头关闭白平衡做识别
clock = time.clock() # Tracks FPS. 设置一个定时器用来计算帧率

click_timer=time.ticks_ms() #计时参数
while True:
    clock.tick()
    img = sensor.snapshot()
    color_detect(img)
    lcd.display(img)
    auto_color_mask(img)
    print(clock.fps())
    if button.state():
        click_timer=time.ticks_ms()          #开始计时
        while button.state():  pass       #等待按键抬起
        if time.ticks_ms()-click_timer>2000: #按键时长超过2s
            break                         #循环退出，回到主界面
    else :
        click_timer=time.ticks_ms()#计时更新
